//StereoGrabber.cpp contains the algorithms for obtaining images for 
//processing from various cameras, including networked cameras and fire wire cameras
#include "StereoGrabber.h"
#include "MyBlob.h"
#include "BlobResult.h"
#include <algorithm>
#include <sstream>

StereoGrabber::StereoGrabber(short LeftCamNum, short RightCamNum, bool ShallCalculateDisparity, bool ShallCalculateDepthMap, bool ShallCalculatePoint3D, bool ShallFindBlobs):
m_LeftCamNum(LeftCamNum),
m_RightCamNum(RightCamNum),
m_ShallCalculateDisparity(ShallCalculateDisparity),
m_ShallCalculateDepthMap(ShallCalculateDepthMap),
m_ShallCalculatePoint3D(ShallCalculatePoint3D),
m_ShallFindBlobs(ShallFindBlobs),
m_LeftCaptureDevice(NULL),
m_RightCaptureDevice(NULL),
m_Q(NULL),
m_Q03(0),
m_Q13(0),
m_Q23(0),
m_Q32(0),
m_Q33(0),
m_M1(NULL),
m_mx1(NULL),
m_my1(NULL),
m_mx2(NULL),
m_my2(NULL),
m_BMState(NULL),
m_LeftGrayView(NULL),
m_LeftGrayUndistordedView(NULL),
m_RightGrayView(NULL),
m_RightGrayUndistordedView(NULL),
m_Disparity(NULL),
m_Focal(0),
m_Baseline(5),
m_K1(-0.004),
m_K2(0.51923),
m_K3(-15.237),
m_K4(166.869),
m_MinAreaValue(500),
m_MaxAreaValue(5000),
m_LeftView(NULL),
m_RightView(NULL),
m_NormalizedDisparity(NULL),
m_DepthMap(NULL),
m_Point3Ds(NULL)
{
	m_M1 = (CvMat *)cvLoad("./Setup/M1.xml",NULL,NULL,NULL);
	m_mx1 = (CvMat *)cvLoad("./Setup/mx1.xml",NULL,NULL,NULL);
	m_my1 = (CvMat *)cvLoad("./Setup/my1.xml",NULL,NULL,NULL);
	m_mx2 = (CvMat *)cvLoad("./Setup/mx2.xml",NULL,NULL,NULL);
	m_my2 = (CvMat *)cvLoad("./Setup/my2.xml",NULL,NULL,NULL);

	m_BMState = cvCreateStereoBMState();

	m_BMState->preFilterSize = 5;
	m_BMState->preFilterCap = 63;
	m_BMState->SADWindowSize = 15;
	m_BMState->minDisparity = 7;
	m_BMState->numberOfDisparities = 64;
	m_BMState->textureThreshold = 0;
	m_BMState->uniquenessRatio = 40;
	m_BMState->speckleWindowSize = 0;
	m_BMState->speckleRange = 0;

	m_Q = (CvMat *)cvLoad("./Setup/Q.xml",NULL,NULL,NULL);
	m_Q03 = cvmGet(m_Q, 0, 3);
	m_Q13 = cvmGet(m_Q, 1, 3);
	m_Q23 = cvmGet(m_Q, 2, 3);
	m_Q32 = cvmGet(m_Q, 3, 2);
	m_Q33 = cvmGet(m_Q, 3, 3);

	m_Focal = cvmGet(m_M1,0,0);
}

StereoGrabber::~StereoGrabber()
{
	stereoGrabberStopCam();
}

void StereoGrabber::stereoGrabberInitFrames()
{
	m_LeftCaptureDevice = cvCaptureFromCAM(m_LeftCamNum);
	assert(m_LeftCaptureDevice!=NULL);

	m_RightCaptureDevice = cvCaptureFromCAM(m_RightCamNum);
	assert(m_RightCaptureDevice!=NULL);

	//assure capture size is correct...
	cvSetCaptureProperty(m_LeftCaptureDevice,CV_CAP_PROP_FRAME_WIDTH,WIDTH);
	cvSetCaptureProperty(m_LeftCaptureDevice,CV_CAP_PROP_FRAME_HEIGHT,HEIGHT);
	cvSetCaptureProperty(m_RightCaptureDevice,CV_CAP_PROP_FRAME_WIDTH,WIDTH);
	cvSetCaptureProperty(m_RightCaptureDevice,CV_CAP_PROP_FRAME_HEIGHT,HEIGHT);

	if(m_ShallCalculateDisparity || m_ShallCalculateDepthMap || m_ShallCalculatePoint3D)
	{
		CvSize size = cvSize(WIDTH,HEIGHT);
		m_LeftGrayView = cvCreateImage( size, IPL_DEPTH_8U, 1 );
		m_LeftGrayUndistordedView = cvCreateImage( size, IPL_DEPTH_8U, 1 );

		m_RightGrayView = cvCreateImage( size, IPL_DEPTH_8U, 1 );
		m_RightGrayUndistordedView = cvCreateImage( size, IPL_DEPTH_8U, 1 );

		m_Disparity = cvCreateImage( size, IPL_DEPTH_16S, 1 );
		m_NormalizedDisparity = cvCreateImage( size, IPL_DEPTH_8U, 1 );

		if(m_ShallCalculateDepthMap)
		{
			m_DepthMap = cvCreateImage( size, IPL_DEPTH_32F, 1 );
		}

		if(m_ShallCalculatePoint3D)
		{
			m_Point3Ds = cvCreateImage( size, IPL_DEPTH_32F, 3 );
		}
	}
}

void StereoGrabber::stereoGrabberGrabFrames()
{
	//Get Camera View
	m_LeftView = cvQueryFrame(m_LeftCaptureDevice);
	m_RightView = cvQueryFrame(m_RightCaptureDevice);

	stereoCalculateDisparity();

}

void StereoGrabber::stereoCalculateDisparity()
{
	//Convert to GrayScale
	cvCvtColor(m_LeftView,m_LeftGrayView,CV_RGB2GRAY);
	cvCvtColor(m_RightView,m_RightGrayView,CV_RGB2GRAY);

	//Apply calibration matrix
	cvRemap(m_LeftGrayView, m_LeftGrayUndistordedView, m_mx1, m_my1);
	cvRemap(m_RightGrayView, m_RightGrayUndistordedView, m_mx2, m_my2);

	//calculate the disparity
	cvFindStereoCorrespondenceBM( m_LeftGrayUndistordedView, m_RightGrayUndistordedView, m_Disparity, m_BMState);

	//Normalize the result
	cvNormalize( m_Disparity, m_NormalizedDisparity, 0, 255, CV_MINMAX, NULL );
}

void StereoGrabber::stereoCalculateDepthMap()
{
	double 	depth = 0;

	for(int x = 0; x < HEIGHT; x++)
	{
		for(int y = 0; y < WIDTH; y++)
		{
			if(cvGet2D(m_NormalizedDisparity,x,y).val[0]>0)
			{
				depth = (double)((m_Baseline*m_Focal)/((double)(cvGet2D(m_Disparity,x,y).val[0]/16)));
				depth = (m_K1*depth*depth*depth + m_K2*depth*depth + m_K3*depth + m_K4);
			}
			else
				depth=0;

			CvScalar DepthScalar = cvGet2D(m_DepthMap,x,y);
			DepthScalar.val[0]= depth;

			cvSet2D(m_DepthMap,x,y,DepthScalar);
		}
	}
}

void StereoGrabber::stereoCalculate3D()
{
	double px, py, pz;

	for (int x = 0; x < HEIGHT; x++)
	{
		for (int y = 0; y < WIDTH; y++)
		{
			if ( cvGet2D(m_NormalizedDisparity,x,y).val[0] == 0 )
			{
				CvScalar s = cvGet2D(m_Point3Ds,x,y);

				s.val[0] = 0;
				s.val[1] = 0;
				s.val[2] = 0;

				cvSet2D(m_Point3Ds,x,y,s);
			}

			uchar d =  cvGet2D(m_Disparity,x,y).val[0];
			double pw = -1.0 * static_cast<double>(d) * m_Q32 + m_Q33;
			px = static_cast<double>(x) + m_Q03;
			py = static_cast<double>(y) + m_Q13;
			pz = m_Q23;

			CvScalar s = cvGet2D(m_Point3Ds,x,y);

			s.val[0] = px/pw;
			s.val[1] = py/pw;
			s.val[2] = pz/pw;

			cvSet2D(m_Point3Ds,x,y,s);
		}
	}
}

double StereoGrabber::CalculateDepth(int x, int y)
{
	double 	depth = 0;

	if(cvGet2D(m_NormalizedDisparity,x,y).val[0]>0)
	{
		depth = (double)((m_Baseline*m_Focal)/((double)(cvGet2D(m_Disparity,x,y).val[0]/16)));
		depth = (m_K1*depth*depth*depth + m_K2*depth*depth + m_K3*depth + m_K4);
	}
	else
		depth=0;

	return depth;
}

CvScalar StereoGrabber::Calculate3D(int x, int y)
{
	double px,py,pz;

	if ( cvGet2D(m_NormalizedDisparity,x,y).val[0] == 0 )
	{
		CvScalar s = cvGet2D(m_Point3Ds,x,y);

		s.val[0] = 0;
		s.val[1] = 0;
		s.val[2] = 0;

		return s;
	}

	uchar d =  cvGet2D(m_Disparity,x,y).val[0];
	double pw = -1.0 * static_cast<double>(d) * m_Q32 + m_Q33;
	px = static_cast<double>(y) + m_Q03;
	py = static_cast<double>(x) + m_Q13;
	pz = m_Q23;

	CvScalar s = cvGet2D(m_Point3Ds,x,y);

	s.val[0] = px/pw;
	s.val[1] = py/pw;
	s.val[2] = pz/pw;

	return s;
}

std::vector<MyBlob*> StereoGrabber::stereoFindBlobs()
{
	std::vector<MyBlob *> DetectedBlobs;
	CBlobResult CurrentImageBlobs;
	CBlob *currentBlob;
	CvPoint rect1, rect2, rect, rect_dist;

	CurrentImageBlobs = CBlobResult(m_NormalizedDisparity, NULL, 0);

	//Remove blobs if it does not cover minimum area specified below
	CurrentImageBlobs.Filter( CurrentImageBlobs, B_EXCLUDE, CBlobGetArea(),B_OUTSIDE,m_MinAreaValue,m_MaxAreaValue);

	CvFont font;
	cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1, 1, 0, 1.4f, CV_AA);

	int detected = 0;

	for (int i = 0; i < CurrentImageBlobs.GetNumBlobs(); i++ )
	{
		currentBlob = CurrentImageBlobs.GetBlob(i);

		double MeanDepth = (double)((m_Baseline*m_Focal)/((double)(currentBlob->Mean(m_Disparity)/16)));
		MeanDepth = (m_K1*MeanDepth*MeanDepth*MeanDepth + m_K2*MeanDepth*MeanDepth + m_K3*MeanDepth + m_K4);

		int decalage = -0.302197 * MeanDepth + 38.7;

		double BlobMinX = (currentBlob->MinX()- decalage - 160) * (MeanDepth / m_Focal);
		double BlobMaxX = (currentBlob->MaxX()- decalage - 160) * (MeanDepth / m_Focal);

		MyBlob * detectedBlob = new MyBlob(BlobMinX, MeanDepth, BlobMaxX, MeanDepth);

		if(detectedBlob->isValid())
		{
			DetectedBlobs.push_back(detectedBlob);

			/*currentBlob->FillBlob( m_LeftView, CV_RGB(0,255,0));

			std::stringstream blobstream;
			blobstream << "Object " << detected+1;
			std::string blobNumObj = blobstream.str();

			std::stringstream distacestream;
			distacestream << MeanDepth << " cm";
			std::string distance = distacestream.str();

			CvBox2D Elipse = currentBlob->GetEllipse();

			cvPutText(m_LeftView, &blobNumObj[0], cvPoint(Elipse.center.x - 30, Elipse.center.y -5), &font, cvScalar(0, 0, 255, 0));
			cvPutText(m_LeftView, &distance[0], cvPoint(Elipse.center.x - 30, Elipse.center.y + 5), &font, cvScalar(0, 0, 255, 0));*/

			detected++;
		}

	}

	return DetectedBlobs;
}

void StereoGrabber::stereoGrabberStopCam()	
{
	cvReleaseCapture( &m_LeftCaptureDevice );
	cvReleaseCapture( &m_RightCaptureDevice );
}
